/****************************************************************************************************************
简介：实现Mega2560扩展版实现舵机库，实现控制1个或者多个(11个)舵机控制.
程序版本0.3
日期：2025.03.10
QQ交流群：811348489
寄语：代码中能遇见你，真好，有你，真好.
署名：有你同创智能机器人科技(北京)有限公司
****************************************************************************************************************/

/**********************************************************************************************************
软件：Arduino2.3.4
硬件：Mega2560，Mega2560扩展版
Mega2560扩展板丝印：
 _______________________________________________________________________________________________________________
|  Motor     |  丝印标签    |    对应程序标签    |                     对应Mega2560引脚号                          \
|            |_____________|___________________|_________________________________________________________________|
|            |     M1      |        1          |电机方向1:37  电机方向2:36  电机pwm:8   编码器A相:18  编码器B相:32  |
|            |     M2      |        2          |电机方向1:A2  电机方向2:A2  电机pwm:5   编码器A相:19  编码器B相:38  |
|            |     M3      |        3          |电机方向1:34  电机方向2:35  电机pwm:12  编码器A相:20  编码器B相:33  |
|            |     M4      |        4          |电机方向1:43  电机方向2:42  电机pwm:9   编码器A相:21  编码器B相:39  |
|            |     M5      |        5          |电机方向1:31  电机方向2:30  电机pwm:6   编码器A相:3   编码器B相:41  |
|            |     M6      |        6          |电机方向1:49  电机方向2:48  电机pwm:4   编码器A相:2   编码器B相:40  /
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|  Servo     |     D2      |        无         |                            2                                    |
|            |     D3      |        无         |                            3                                    |
|            |     D4      |        无         |                            4                                    |
|            |     D6      |        无         |                            6                                    |
|            |     D44     |        无         |                            44                                   |
|            |     D45     |        无         |                            45                                   |
|            |     D46     |        无         |                            46                                   |
|            |     D7      |        无         |                            7                                    |
|            |     D10     |        无         |                            10                                   |
|            |     D11     |        无         |                            11                                   |
|            |     D13     |        无         |                            13                                   /
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|Ultrasonic1 |     5V      |        无         |                            5V                                   |
|            |     D23     |        无         |                            23                                   |
|            |     D22     |        无         |                            22                                   |
|            |     GND     |        无         |                            GND                                  |
|            |￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|Ultrasonic2 |     5V      |        无         |                            5V                                   |
|            |     D25     |        无         |                            25                                   |
|            |     D24     |        无         |                            24                                   |
|            |     GND     |        无         |                            GND                                  |
|            |￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|Ultrasonic3 |     5V      |        无         |                            5V                                   |
|            |     D27     |        无         |                            27                                   |
|            |     D26     |        无         |                            26                                   |
|            |     GND     |        无         |                            GND                                  |
|            |￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣
|Ultrasonic4 |     5V      |        无         |                            5V                                   |
|            |     D29     |        无         |                            29                                   |
|            |     D28     |        无         |                            28                                   |
|            |     GND     |        无         |                            GND                                  |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
| PS2        |     GND     |        无         |                            GND                                  |
|            |     5V      |        无         |                            5V                                   |
|            |     52      |        无         |                            SCK                                  |
|            |     51      |        无         |                            MOSI                                 |
|            |     53      |        无         |                            SS                                   |
|            |     50      |        无         |                            MISO                                 |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|Line sensor |     5V      |        无         |                            5V                                   |
|            |     GND     |        无         |                            GND                                  |
|            |     l       |        无         |                            A12                                  |
|            |     2       |        无         |                            A13                                  |
|            |     3       |        无         |                            A14                                  |
|            |     4       |        无         |                            A15                                  |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|COM3        |     5V      |        无         |                            5V                                   |
|            |     RX3     |        无         |                            RX3                                  |
|            |     TX3     |        无         |                            TX3                                  |
|            |     GND     |        无         |                            GND                                  |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|COM2        |     5V      |        无         |                            5V                                   |
|            |     RX2     |        无         |                            RX2                                  |
|            |     TX2     |        无         |                            TX2                                  |
|            |     GND     |        无         |                            GND                                  |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|IIC         |     5V      |        无         |                            5V                                   |
|            |     SCL     |        无         |                            SCL                                  |
|            |     SDA     |        无         |                            SDA                                  |
|            |     GND     |        无         |                            GND                                  |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|Stepper     |     EN      |        无         |                            30                                   |
|            |     MS1     |        无         |                            31                                   |
|            |     MS2     |        无         |                            49                                   |
|            |     MS3     |        无         |                            48                                   |
|            |     STEP    |        无         |                            2                                    |
|            |     DIR     |        无         |                            40                                   |
￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣

**********************************************************************************************************/
#ifndef _UNI_SERVO_H_
#define _UNI_SERVO_H_

#include<Arduino.h>
#include<Servo.h>

#define SERVO_LIB_VERSION 0.3//库函数版本(修改部分程序结构)

//#################################################################################
//#####  舵机库函数类定义部分  ######################################################
//#################################################################################
#define SERVO_MAX_COUNT 11    //定义舵机最大数量
const uint8_t servo_pin[11] = {2,3,4,6,44,45,46,7,10,11,13};


class UNI_SERVO{
  public:
    //构造函数，声明1个舵机
    UNI_SERVO(uint8_t _pin1);
    //构造函数，声明2个舵机
    UNI_SERVO(uint8_t _pin1, uint8_t _pin2);
    //构造函数，声明3个舵机
    UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3);
    //构造函数，声明4个舵机
    UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4);
    //构造函数，声明5个舵机
    UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5);
    //构造函数，声明6个舵机
    UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6);
    //构造函数，声明7个舵机
    UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6, uint8_t _pin7);
    //构造函数，声明8个舵机
    UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6, uint8_t _pin7, uint8_t _pin8);
    //构造函数，声明9个舵机
    UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6, uint8_t _pin7, uint8_t _pin8, uint8_t _pin9);
    //构造函数，声明10个舵机
    UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6, uint8_t _pin7, uint8_t _pin8, uint8_t _pin9, uint8_t _pin10);
    //构造函数，声明11个舵机
    UNI_SERVO(uint8_t _pin1, uint8_t _pin2, uint8_t _pin3, uint8_t _pin4, uint8_t _pin5, uint8_t _pin6, uint8_t _pin7, uint8_t _pin8, uint8_t _pin9, uint8_t _pin10, uint8_t _pin11);
    //析构函数，目前该函数功能暂未开放，后续开发可能会添加该函数功能
    ~UNI_SERVO();

    //使能舵机
    void uni_servo_attach_start();
    //舵机初始化
    void init();

    //功能：指定舵机编号的舵机转动函数（使用于180度舵机）
    void pin_move180(int label1, int angle);

    //270舵机底层转动函数（使用于180度舵机）
    void pin_move270(int servo_pin, float angle);

    //功能：指定舵机编号的舵机转动函数（舵机通用）
    void pin_movemap(int servo_pin, float angle);

    //=========================================================================================================================
    //================== 180度舵机转动函数 =====================================================================================
    //=========================================================================================================================
    //定义的1个舵机，转动到ang1角度
    void move180(int ang1);
    //定义的2个舵机，分别转动到ang1,ang2角度
    void move180(int ang1, int ang2);
    //定义的3个舵机，分别转动到ang1,ang2,ang3角度
    void move180(int ang1, int ang2, int ang3);
    //定义的4个舵机，分别转动到ang1,ang2,ang3,ang4角度
    void move180(int ang1, int ang2, int ang3, int ang4);
    //定义的5个舵机，分别转动到ang1,ang2,ang3,ang4,ang5角度
    void move180(int ang1, int ang2, int ang3, int ang4, int ang5);
    //定义的6个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6角度
    void move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6);
    //定义的7个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7角度
    void move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7);
    //定义的8个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8角度
    void move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8);
    //定义的9个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8,ang9角度
    void move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9);
    //定义的10个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8,ang9,ang10角度
    void move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10);
    //定义的11个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8,ang9,ang10,ang11角度
    void move180(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10, int ang11);

    //=========================================================================================================================
    //================== 270度舵机转动函数 =====================================================================================
    //=========================================================================================================================
    //定义的1个舵机，转动到ang1角度
    void move270(int ang1);
    //定义的2个舵机，分别转动到ang1,ang2角度
    void move270(int ang1, int ang2);
    //定义的3个舵机，分别转动到ang1,ang2,ang3角度
    void move270(int ang1, int ang2, int ang3);
    //定义的4个舵机，分别转动到ang1,ang2,ang3,ang4角度
    void move270(int ang1, int ang2, int ang3, int ang4);
    //定义的5个舵机，分别转动到ang1,ang2,ang3,ang4,ang5角度
    void move270(int ang1, int ang2, int ang3, int ang4, int ang5);
    //定义的6个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6角度
    void move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6);
    //定义的7个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7角度
    void move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7);
    //定义的8个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8角度
    void move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8);
    //定义的9个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8,ang9角度
    void move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9);
    //定义的10个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8,ang9,ang10角度
    void move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10);
    //定义的11个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8,ang9,ang10,ang11角度
    void move270(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10, int ang11);

    //=========================================================================================================================
    //================== 舵机500~2500脉宽转动函数 ===============================================================================
    //=========================================================================================================================
    //定义的1个舵机，转动到ang1角度
    void movemap(int ang1);
    //定义的2个舵机，分别转动到ang1,ang2角度
    void movemap(int ang1, int ang2);
    //定义的3个舵机，分别转动到ang1,ang2,ang3角度
    void movemap(int ang1, int ang2, int ang3);
    //定义的4个舵机，分别转动到ang1,ang2,ang3,ang4角度
    void movemap(int ang1, int ang2, int ang3, int ang4);
    //定义的5个舵机，分别转动到ang1,ang2,ang3,ang4,ang5角度
    void movemap(int ang1, int ang2, int ang3, int ang4, int ang5);
    //定义的6个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6角度
    void movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6);
    //定义的7个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7角度
    void movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7);
    //定义的8个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8角度
    void movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8);
    //定义的9个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8,ang9角度
    void movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9);
    //定义的10个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8,ang9,ang10角度
    void movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10);
    //定义的11个舵机，分别转动到ang1,ang2,ang3,ang4,ang5,ang6,ang7,ang8,ang9,ang10,ang11角度
    void movemap(int ang1, int ang2, int ang3, int ang4, int ang5, int ang6, int ang7, int ang8, int ang9, int ang10, int ang11);

    //=========================================================================================================================
    //下一版本将添加360度圆周舵机，敬请期待 ======================================================================================
    //=========================================================================================================================

    //指定_pin引脚的舵机失能
    void pin_detach(uint8_t _pin);
    //所有舵机都失能
    void detach();

    //串口打印舵机定义的参数
    void get_parameters();

  private:
    Servo _servos[11];
    uint8_t _uni_servo_label[11];   //记录舵机标签数组
    int _uni_servo_current_count = 0;    //记录舵机数量
    int _uni_servo_pin[SERVO_MAX_COUNT];   //记录舵机引脚数组
};

#endif

